clc;
clear;

%% main主脚本
tic;
startTime = cputime;
pde = process_pde_NS_s(3);
vector_h = [1/4 1/8 1/16 1/32];
basis_type1 = "P2";
basis_type2 = "P1";
iter_L = 8;
iter_N = 8;
ka = 0.5;
rho = 0.5*2*pde.nu*(1-ka);

u_Linf = zeros(size(vector_h,2),1);
u_L2 = zeros(size(vector_h,2),1);
u_H1 = zeros(size(vector_h,2),1);
p_Linf = zeros(size(vector_h,2),1);
p_L2 = zeros(size(vector_h,2),1);
p_H1 = zeros(size(vector_h,2),1);

for i = 1:size(vector_h,2)
    h = vector_h(i);
    mesh = process_mesh(pde, h);
    fs1 = process_fespace(mesh, basis_type1);
    fs2 = process_fespace(mesh, basis_type2);
    shift_p = 2*fs1.Nb;

    % process_showmesh(mesh, fs1);
    % process_showmesh(mesh, fs2);

    % stokes for initial
    A1 = pde.nu*term_grad_U_grad_V(mesh, fs1);
    % A2 = term_P_divV(mesh, fs1, fs2);
    % A0 = sparse(fs2.Nb, fs2.Nb);
    % A = [A1, -A2;
    %     -A2', A0];
    b1 = term_F_V(mesh, fs1, pde.f1, pde.f2);
    % b0 = sparse(fs2.Nb, 1);
    % b = [b1; b0];
    % [A, b] = process_boundary_NS_s(A, b, mesh, fs1, fs2, pde);
    % A(shift_p+1,:) = 0;
    % A(shift_p+1,shift_p+1) = 1;
    % b(shift_p+1,1) = pde.p(pde.domain(1), pde.domain(3));
    % U_old = A\b;
    % U_n = U_old(1:shift_p,1);
    % P_n = U_old(shift_p+1:end,1);

    % test
    U_n = [pde.u1(fs1.node(:,1), fs1.node(:,2));
        pde.u2(fs1.node(:,1), fs1.node(:,2))];
    P_n = [pde.p(fs2.node(:,1), fs2.node(:,2))];
    for l = 1:iter_L
        bu = term_Pold_div_V(mesh, fs1, fs2, P_n);
        for n = 1:iter_N
            AN1 = term_Uold_grad_U_V(mesh, fs1, U_n);
            AN2 = term_Uold_grad_V_U(mesh, fs1, U_n);
            An = A1 + 0.5*(AN1-AN2);
            bn = b1 + bu;
            for k = 1:size(pde.bdary,2)
                if 0 == pde.bdary(1,k)
                    bdn1 = bdary_node(mesh, fs1, k);
                    An(bdn1,:) = 0;
                    An(sub2ind(size(An), bdn1, bdn1)) = 1;
                    bn(bdn1) = pde.u1(fs1.node(bdn1,1),fs1.node(bdn1,2));
                    bdn2 = bdn1+fs1.Nb;
                    An(bdn2,:) = 0;
                    An(sub2ind(size(An), bdn2, bdn2)) = 1;
                    bn(bdn2) = pde.u2(fs1.node(bdn1,1),fs1.node(bdn1,2));
                end
            end
            U_n = An\bn;
        end
        Ap = term_P_Q(mesh, fs2);
        bp1 = term_div_Uold_Q(mesh, fs1, fs2, U_n);
        bp = (Ap*P_n-rho*bp1);
        Ap(1,:) = 0;
        Ap(1,1) = 1;
        bp(1) = pde.p(pde.domain(1), pde.domain(3));
        P_n = Ap\bp;
        U = [U_n; P_n];
    end

    % process_plot(mesh, fs1, fs2, U);
    process_field(fs1, fs2, U);

    [u_Linf(i,1), p_Linf(i,1)] = process_error_NS_s(mesh, fs1, fs2, pde, U, "Linf");
    [u_L2(i,1), p_L2(i,1)] = process_error_NS_s(mesh, fs1, fs2, pde, U, "L2");
    [u_H1(i,1), p_H1(i,1)] = process_error_NS_s(mesh, fs1, fs2, pde, U, "H1");

    fprintf("已完成\t%d/%d\n", i, size(vector_h,2));
end

process_rate(u_Linf, vector_h);
process_rate(u_L2, vector_h);
process_rate(u_H1, vector_h);
process_rate(p_Linf, vector_h);
process_rate(p_L2, vector_h);
process_rate(p_H1, vector_h);

TOCtime = toc;
CPUtime = cputime - startTime;
disp(["TOC time(s)", num2str(TOCtime)]);
disp(["CPU time(s)", num2str(CPUtime)]);
